
package miss.sheffield;

/**
 *
 * @author Kevin Doyle
 */
public class GenericVencentyAlgorithm{

    /**
     *
     * @param <T>
     * @param array
     * @return
     */
    private static <T> void forwardAzimuth( T x) {
        T fwAzimuth = x;
        double a = 6378160D;
        double b = 6356774.7189999996D;
        double f = (a - b) / a;
  //      double L = Math.toRadians(longitudeB - longitudeA);
  //      double U1 = Math.atan((1.0D - f) * Math.tan(Math.toRadians(latitudeA)));
   //     double U2 = Math.atan((1.0D - f) * Math.tan(Math.toRadians(latitudeB)));
    //    double sinU1 = Math.sin(U1);
    //    double cosU1 = Math.cos(U1);
    //    double sinU2 = Math.sin(U2);
   //     double cosU2 = Math.cos(U2);
    //    double lambda = L;
        double iterLimit = 100D;
        double sinLambda = Math.sin(lambda);
        double cosLambda = Math.cos(lambda);
        double sinSigma = Math.sqrt(cosU2 * sinLambda * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
        double cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
        double sigma = Math.atan2(sinSigma, cosSigma);
        double sinAlpha = (cosU1 * cosU2 * sinLambda) / sinSigma;
        double cosSqAlpha = 1.0D - sinAlpha * sinAlpha;
        double cos2SigmaM = cosSigma - (2D * sinU1 * sinU2) / cosSqAlpha;
        double lambdaP;
        do {
            sinLambda = Math.sin(lambda);
            cosLambda = Math.cos(lambda);
            sinSigma = Math.sqrt(cosU2 * sinLambda * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
            if (sinSigma == 0.0D) {
                System.out.println("co-incident points");
            }
            cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
            sigma = Math.atan2(sinSigma, cosSigma);
            sinAlpha = (cosU1 * cosU2 * sinLambda) / sinSigma;
            cosSqAlpha = 1.0D - sinAlpha * sinAlpha;
            cos2SigmaM = cosSigma - (2D * sinU1 * sinU2) / cosSqAlpha;
            if (Double.isNaN(cos2SigmaM)) {
                cos2SigmaM = 0.0D;
            }
            double C = (f / 16D) * cosSqAlpha * (4D + f * (4D - 3D * cosSqAlpha));
            lambdaP = lambda;
            lambda = L + (1.0D - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1D + 2D * cos2SigmaM * cos2SigmaM)));
        } while (Math.abs(lambda - lambdaP) > 9.9999999999999998E-013D && --iterLimit > 0.0D);
        if (iterLimit == 0.0D) {
            System.out.println("NaN iterLimit==0 \n formula failed to converge");
        }
        double fwdAz = Math.atan2(cosU2 * sinLambda, cosU1 * sinU2 - sinU1 * cosU2 * cosLambda);
      // return fwAzimuth;
       //Math.toDegrees(fwdAz);
       // return null;
    }

    public static double getGroundRange(double latitudeA, double longitudeA, double latitudeB, double longitudeB) {
        double a = 6378160D;
        double b = 6356774.7189999996D;
        double f = (a - b) / a;
        double L = Math.toRadians(longitudeB - longitudeA);
        double U1 = Math.atan((1.0D - f) * Math.tan(Math.toRadians(latitudeA)));
        double U2 = Math.atan((1.0D - f) * Math.tan(Math.toRadians(latitudeB)));
        double sinU1 = Math.sin(U1);
        double cosU1 = Math.cos(U1);
        double sinU2 = Math.sin(U2);
        double cosU2 = Math.cos(U2);
        double lambda = L;
        double iterLimit = 100D;
        double sinLambda = Math.sin(lambda);
        double cosLambda = Math.cos(lambda);
        double sinSigma = Math.sqrt(cosU2 * sinLambda * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
        double cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
        double sigma = Math.atan2(sinSigma, cosSigma);
        double sinAlpha = (cosU1 * cosU2 * sinLambda) / sinSigma;
        double cosSqAlpha = 1.0D - sinAlpha * sinAlpha;
        double cos2SigmaM = cosSigma - (2D * sinU1 * sinU2) / cosSqAlpha;
        double lambdaP;
        do {
            sinLambda = Math.sin(lambda);
            cosLambda = Math.cos(lambda);
            sinSigma = Math.sqrt(cosU2 * sinLambda * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
            if (sinSigma == 0.0D) {
                System.out.println("co-incident points");
            }
            cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
            sigma = Math.atan2(sinSigma, cosSigma);
            sinAlpha = (cosU1 * cosU2 * sinLambda) / sinSigma;
            cosSqAlpha = 1.0D - sinAlpha * sinAlpha;
            cos2SigmaM = cosSigma - (2D * sinU1 * sinU2) / cosSqAlpha;
            if (Double.isNaN(cos2SigmaM)) {
                cos2SigmaM = 0.0D;
            }
            double C = (f / 16D) * cosSqAlpha * (4D + f * (4D - 3D * cosSqAlpha));
            lambdaP = lambda;
            lambda = L + (1.0D - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1D + 2D * cos2SigmaM * cos2SigmaM)));
        } while (Math.abs(lambda - lambdaP) > 9.9999999999999998E-013D && --iterLimit > 0.0D);
        if (iterLimit == 0.0D) {
            System.out.println("NaN iterLimit==0 \n formula failed to converge");
        }
        double uSq = (cosSqAlpha * (a * a - b * b)) / (b * b);
        double A = 1.0D + (uSq / 16384D) * (4096D + uSq * (-768D + uSq * (320D - 175D * uSq)));
        double B = (uSq / 1024D) * (256D + uSq * (-128D + uSq * (74D - 47D * uSq)));
        double deltaSigma = B * sinSigma * (cos2SigmaM + (B / 4D) * (cosSigma * (-1D + 2D * cos2SigmaM * cos2SigmaM) - (B / 6D) * cos2SigmaM * (-3D + 4D * sinSigma * sinSigma) * (-3D + 4D * cos2SigmaM * cos2SigmaM)));
        double s = b * A * (sigma - deltaSigma);
        return s;
    }
}
